
/*
 * Include Files
 *
 */
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#define SIMPLIFIED_RTWTYPES_COMPATIBILITY
#include "rtwtypes.h"
#undef SIMPLIFIED_RTWTYPES_COMPATIBILITY
#endif



/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#include <math.h>

#define     wcc         500.0  //hz
#define     Rs          0.28
#define     Ls          0.0002
#define     Kp          (Ls * 6.28 * wcc)
#define     Ki          (Rs * 6.28 * wcc * 0.0001)
#define     Kerr        (0.5 * Ki + Kp)
#define     KerrOld     (0.5 * Ki - Kp)
#define     BemfK       0.2   // V/Hz
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
#define u_width 1
#define u_1_width 1
#define u_2_width 1
#define u_3_width 1
#define y_width 1
#define y_1_width 1

/*
 * Create external references here.  
 *
 */
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
 
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */

/*
 * Output function
 *
 */
void current_control_Outputs_wrapper(const real32_T *d_error,
			const real32_T *q_error,
			const real32_T *we,
			const uint32_T *cnt,
			real32_T *d_out,
			real32_T *q_out)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
static float carryOver_d;
    static float carryOver_q;
    static float preWe;
    
    if(*cnt < 2000)
    {
        *q_out = 0;
        carryOver_q = 0;
    }
    else
    {
        *q_out += ((*q_error * Kerr) + carryOver_q);
        carryOver_q = *q_error * KerrOld + BemfK * (*we - preWe);
    }
    *d_out += ((*d_error * Kerr) + carryOver_d);
    carryOver_d = *d_error * KerrOld;
    preWe = *we;
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}


